#include <joyos.h>
#include <happylib.h>

// my headers
#include "umain.h"
#include "define.h"
#include "goalscore.h"
#include "robomove.h"
#include "roboturn.h"
#include "led.h"
#include "bumper.h"
#include "distance.h"
#include "init.h"
#include "skunk.h"
#include "strategy.h"
#include "robotest.h"
#include "fantasy.h"

/*************************************************************
 * TESTING AREA
 */


int usetup(void) {
	//caliUnderLed();
	printf("\nCalibrate Gyro");
	calibrateGyro();
	set_auto_halt(0);
	return 0;
}

int umain (void) {
	pointToDo[0] = 4;
	pointToDo[1] = 4;
	pointToDo[2] = 4;
	initPlace = 0;
	initOrient = 1;
	
	caliLeftLed();
	state = 3;
	
	printf_P (PSTR("\nClick to Rock!"));
	go_click();
	
	myStrategy();
	
	return 0;
}



// usetup is called during the calibration period. It must return before the
// period ends.
/*
int usetup (void) {
	
	printf_P (PSTR("\nHello My Boss!"));
	go_click();
	printf_P (PSTR("\nBattery Voltage: %d mV"), read_battery());
	go_click();
	
	// Point Input
	getGoalScore(); // also modify pointToDo
	printf_P (PSTR("\nScore Set: %d"), goalPoint);
	go_click();
	
	// Calibrate Goal and Skunk
	//calibrateGoalServo();
	//pause(1000);
	//calibrateSkunkServo();
	//pause(1000);
	
	// Under IR/LED
	caliUnderLed();
	pause(2000);
	caliLeftLed();
	//pause(1000);
	
	caliRightLed();
	//pause(1000);
	
	// Gyro
	printf_P (PSTR("\nCalibrate Gyro"));	
	go_click ();
	calibrateGyro();
	
	return 0;
}
*/

// Entry point to contestant code.
// Create threads and return 0.
/*
int umain (void) {
	action();
	return 0;
}
*/
/*int umain (void) {
	//fantasy();
	//roboTest();
	action();
	
	pointToDo[0] = 4;
	pointToDo[1] = 4;
	pointToDo[2] = 4;
	initPlace = 0;
	initOrient = 1;
	state = 1;
	caliLeftLed();
	printf("\nGGGGG");
	go_click();
	myStrategy();
	
	
	initPose();
	initPlace = 0;
	initOrient = 1;
	state = 1;
	myStrategy();
	
	return 0;
}*/




void action(void){
	printf_P (PSTR("\nClick to Rock!"));
	go_click();
	
	initPose();
	printf_P (PSTR("\nPlace %d, orient %d"), initPlace, initOrient);
	initTurn();
	
	state = 1;
	myStrategy();
}
